"""
车辆参数
读取conf配置文件中的车身参数
"""
import json
import logging
import math

logger = logging.getLogger(__name__)


class VehicleParams:
    _instance = None

    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super(VehicleParams, cls).__new__(cls)
        return cls._instance

    def __init__(self):
        self.vehicle_params = {
            "length": 1.4,
            "width": 0.94,
            "front_wheel_to_front_bumper": 0.116,
            "front_ego_to_front_bumper": 0.35,
            "rear_ego_to_rear_bumper": 0.12,
            "rear_wheel_to_rear_bumper": 0.1,
            "maximum_angle": 0.5,
            "maximum_velocity": 23.0,
            "maximum_acceleration": 5.0,
            "lf" : 0.71,
            "lr" : 0.49,
            "m" : 125.76,
            "L" : 1.2,
            "Cf" : -2020,
            "Cr" : -2020
        }
        # 以车身轴距中心为基准，若该数为正，则定位中心在车身中心前方，反之在车身中心后方
        self.position_deviation = 0.0

    def load_vehicle_params(self, conf: str = None) -> bool:
        if conf is None:
            conf = "./conf/vehicle_params.json"
            logger.info("Load vehicle params from default path")
        else:
            logger.info("Load vehicle params from user-defined")
        try:
            self.vehicle_params = json.loads(open(conf, "r").read())

            if self.calculate_axis():
                return True
        except:
            return False
        return False

    def calculate_axis(self) -> bool:
        self.vehicle_params.update({"front_axis_to_rear_axis": self.vehicle_params["length"] - \
                                                               self.vehicle_params["front_wheel_to_front_bumper"] - \
                                                               self.vehicle_params["rear_wheel_to_rear_bumper"]})
        try:
            position_length = self.vehicle_params["length"] - self.vehicle_params["front_ego_to_front_bumper"] - \
                              self.vehicle_params["rear_ego_to_rear_bumper"]
            position_length_to_rear = position_length / 2 + self.vehicle_params["rear_ego_to_rear_bumper"]
            axis_length = self.vehicle_params["front_axis_to_rear_axis"] / 2 + self.vehicle_params[
                "rear_wheel_to_rear_bumper"]
            self.position_deviation = position_length_to_rear - axis_length
            return True
        except:
            return False
